#pragma config(Sensor, in1,    PotArm,              sensorPotentiometer)
#pragma config(Sensor, in2,    DetectTube,          sensorLineFollower)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  BaseSonar,           sensorSONAR_cm)
#pragma config(Sensor, dgtl11, ArmUp,               sensorTouch)
#pragma config(Sensor, dgtl12, ArmDown,             sensorTouch)
#pragma config(Motor,  port1,           DriveRB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           DriveRF,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           DriveLF,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           ArmRL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           ArmLL,         tmotorNormal, openLoop)
#pragma config(Motor,  port6,           ArmRU,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           ArmLU,         tmotorNormal, openLoop)
#pragma config(Motor,  port8,           SuckR,         tmotorNormal, openLoop)
#pragma config(Motor,  port9,           SuckL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          DriveLB,       tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "delta_lib_V2.c" //Main Funtion Librarys
//#include "PIDController.c"
#include "PID Test.c"

//Standard Lock////////
int arm_grounded;
int low_descore;
int low_lock;
int high_descore;
int high_lock;

//PID//////////////////
PIDController arm;

int goal_value = 1000;
int k_P = 1;
int k_I = 0;
int k_D = 0;

bool PID_arm = false;

void pre_auton()
{
  //Encoders:
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;

  //PID:
  init(arm,PotArm,port4);
  setSetpoint(arm, goal_value);
  setPIDs(arm, k_P, k_I, k_D);
  enable(arm);
  //
  arm_grounded = SensorValue[PotArm];       // sets ground point           (0 inches)
  low_descore = arm_grounded + /*1556 - 1247*/ 309; // sets low descore arm point  (4.5 inches)
  low_lock = arm_grounded + /*2326 - 1247*/ 1079;    //...lowgoal                   (15 inches)
  high_descore = arm_grounded + /*1879 - 1247*/ 632; //...high descore              (x inches)
  high_lock = arm_grounded + /*2599 - 1247*/ 1352;   // ...high goal                (18.5 inches)
  arm_grounded += 250;
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
  pre_auton();

  int arm_in_position = 0;  //arm is down; 0 for false and 1 for true

  drive_suck(100,FULL,5);//speed,suckspeed,inches//drive straight and inhale the red stack at the same time

  while(arm_in_position != 1) {
    arm_in_position = lock(low_lock);
  }

  turn(127,-90);//speed,degrees turn left

  while(arm_in_position != 1) {//place tubes
      arm_in_position = lock(low_descore);
  }

  drive(-FULL,5);// back up
  turn(FULL,180); //left or shouldn't matter, turn to face blue stack

  drive_suck(FULL,FULL,10);//drive to blue stack//drive straight and inhale the blue stack at the same time

  turn(FULL,90);//turn right
  drive(FULL,7);//drive straight
  turn(FULL,-45);//turn left to face tower
  drive(FULL,7);//drive to tower
  sucker(-FULL,3);//spit tube into tower
}

task usercontrol()
{
  while (true)
  {
    pre_auton();
    int goal_lock = 0;
    //0 for unlocked, -1 for low gal, 1 for high goal
    //calibration for potentiometer setting the arm
    /*int low_lock_point = SensorValue[in1] + 703;
    //low goal potentiometer reading
    //sets the low lock point based on the starting potentiometer point int high_lock_point = SensorValue[in1] + 1000;
    //high goal potentiometer reading
    //end calibrarion*/
    while(true)
    {
/*
      //Auto_Arm//
      if(vexRT[Btn7D] == 1)//auto button close loop
      {
        goal_lock = -1;   //sets to low lock
      }
      else if(vexRT[Btn7U] == 1) {
        goal_lock = 1;	//sets to high lock
      }
      else if(vexRT[Btn6U] == 1) {// descore/ score on low goal...
          goal_lock = 2;
      }

      if(goal_lock == -1)
        lock(low_lock); //brings to low lock point
      else if(goal_lock == 1)
        lock(high_lock);//moves arm to high lock point

      //NEW STUFF TAKE THIS OUT IF IT DOESNT WORK
      else if (goal_lock == 2)
        lock(low_descore);//moves arm to descore lock point
      else if(goal_lock == 3)
        lock(high_descore);
      //------------------
*/

if(vexRT[Btn7L] == 1)//auto button close loop
  	{
   	goal_lock = -1;   //sets to low lock
  	}
 	else if(vexRT[Btn7U] == 1) {
   	goal_lock = 1;	//sets to high lock
  	}
 	else if(vexRT[Btn7D] == 1) {// descore/ score on low goal...
   	goal_lock = 2;
  	}
 	else if(vexRT[Btn7R] == 1) {//same as 7D...
   	goal_lock = 2;
  	}


  	if(goal_lock == -1)
    	lock(low_lock); //brings to low lock point
  	else if(goal_lock == 1)
    	lock(high_lock);//moves arm to high lock point
  	///////////////////////////////////////////////////////////////NEW STUFF TAKE THIS OUT IF IT DOESNT WORK
  	else if (goal_lock == 2)
    	lock(low_descore);//moves arm to descore lock point
  	else if(goal_lock == 3)
    	lock(high_descore);

      //Manual_Arm//
      if(vexRT[Ch3] < 15 && vexRT[Ch3] > -15){//Trim, if stick is between 15 & negative 15 motors equal 0.
          setArmSpeed(0);
      }
      else{//motors = stick angle
          setArmSpeed(vexRT[Ch3]);
        goal_lock=0;
      }


      //INHALE//
      switch(vexRT[Btn5U] - vexRT[Btn5D])
      {
      case  1:motor[SuckR] = FULL;
        motor[SuckL] = FULL;
        break;
      case -1:motor[SuckR] = -FULL;
        motor[SuckL] = -FULL;
        break;
      case  0:motor[SuckR] = 0;
        motor[SuckL] = 0;
        break;
      }//: switch

      /*
      //PID ARM//
      if (vexRT[ch3] > 5 || vexRT[ch3] < -5) {
      PID_arm;
      setArmSpeed(ch3);
      }
      else if (vexRT[Btn7L] == 1) {
      PID_arm = false;
      setSetpoint(arm, low_lock);
      }
      else if
      (vexRT[Btn7U] == 1) {
      PID_arm;
      setSetpoint(arm, low_descore);
      }
      else if
      (vexRT[Btn7R] == 1) {
      PID_arm;
      setSetpoint(arm, high_descore);
      }
      else if
      (vexRT[Btn7D] == 1) {
      PID_arm;
      setSetpoint(arm, high_lock);
      }
      */
      //Drive_Train//
      setDriveRSpeed((vexRT[Ch2] - vexRT[Ch1]));// (y + x)
      setDriveLSpeed((vexRT[Ch2] + vexRT[Ch1]));// (y - x)
      /*
      if (PID_arm){
      setArmSpeed(calculatePID(arm));
      }
      */
    }
  }
}
